#include <settings.hpp>
#include <utils.hpp>
//
#include <remoter.hpp>
#include <remoter_cali_finished.hpp>

RemoterCaliFinished::RemoterCaliFinished(int state)
	: RemoterCali(state)
{
}

RemoterCaliFinished::~RemoterCaliFinished()
{
}

void RemoterCaliFinished::Process(msg_rc_raw_s* rcRaw, msg_rc_s* rc)
{
	RemoterCali::Process(rcRaw, rc);

	if (m_Tk % m_CaliWaitTk == 0)
	{
		m_Step++;
		LOG("RemoterCalibration: MOVE ZERO. %d.\n", m_Step);
	}

	for (int i = 0; i < MSG_RC_E_CNT; i++)
	{
		m_RcRawCurr[i] = FilterLowPass((double)rcRaw->rc_raw[i], m_RcRawPrev[i], m_CaliFilter);
	}

	if (m_Step >= m_CaliWaitStep)
	{
		g_Settings->rc_cali_thro_mid  = m_RcRawCurr[g_Settings->rc_ind_thro] + 0.5;
		g_Settings->rc_cali_roll_mid  = m_RcRawCurr[g_Settings->rc_ind_roll] + 0.5;
		g_Settings->rc_cali_pitch_mid = m_RcRawCurr[g_Settings->rc_ind_pitch] + 0.5;
		g_Settings->rc_cali_yaw_mid	  = m_RcRawCurr[g_Settings->rc_ind_yaw] + 0.5;

		if (g_Settings->rc_cali_thro_reverse == 0)
		{
			if (g_Settings->rc_cali_thro_mid < g_Settings->rc_cali_thro_min)
			{
				g_Settings->rc_cali_thro_mid = g_Settings->rc_cali_thro_min;
			}
			if (g_Settings->rc_cali_thro_mid > g_Settings->rc_cali_thro_max)
			{
				g_Settings->rc_cali_thro_mid = g_Settings->rc_cali_thro_max;
			}
		}
		else
		{
			if (g_Settings->rc_cali_thro_mid > g_Settings->rc_cali_thro_min)
			{
				g_Settings->rc_cali_thro_mid = g_Settings->rc_cali_thro_min;
			}
			if (g_Settings->rc_cali_thro_mid < g_Settings->rc_cali_thro_max)
			{
				g_Settings->rc_cali_thro_mid = g_Settings->rc_cali_thro_max;
			}
		}

		LOG("RemoterCalibration: Finished.\n");

		Remoter::Instance()->SwitchCaliNext();
	}
}